// 必须添加的头文件：解决termios/read/STDIN_FILENO等未定义问题
#include <termios.h>   // 终端控制结构体termios、tcgetattr/tcsetattr函数
#include <unistd.h>   // read函数、STDIN_FILENO宏
#include <fcntl.h>    // 终端模式相关定义
#include <chrono>     // 时间相关（原代码已用，确保包含）
#include <iostream>   // 打印输出
#include <cstdio>     // printf函数
#include "ugv_sdk/mobile_robot/scout_robot.hpp"  // 小车SDK头文件
#include "ugv_sdk/utilities/protocol_detector.hpp"  // 协议检测头文件

using namespace westonrobot;
using namespace std::chrono;  // 简化时间相关代码的命名空间

// -------------------------- 全局配置参数（按需调整）--------------------------
const float MAX_LINEAR_SPEED = 0.5f;    // 最大线速度（m/s）
const float MAX_ANGULAR_SPEED = 1.0f;   // 最大角速度（rad/s）
const float SPEED_STEP = 0.05f;         // 按键时速度增量
const float DECEL_STEP = 0.05f;         // 松开时减速增量
const int LOOP_INTERVAL_US = 20000;     // 控制循环间隔（20ms）

// 按键状态记录（全局，用于多按键组合检测）
bool is_w_pressed = false;
bool is_s_pressed = false;
bool is_a_pressed = false;
bool is_d_pressed = false;

// -------------------------- 辅助函数定义 --------------------------
// 1. 非阻塞读取按键并更新按键状态（修复终端相关错误）
char GetKeyWithState() {
    struct termios oldt, newt;  // 终端属性结构体（需包含termios.h）
    int ch = 0;

    // 1. 保存终端原有设置（避免影响系统默认终端行为）
    if (tcgetattr(STDIN_FILENO, &oldt) != 0) {  // STDIN_FILENO在unistd.h中定义
        return 0;  // 读取终端属性失败，返回空
    }

    // 2. 设置终端为非阻塞模式（支持持续按键检测）
    newt = oldt;
    newt.c_lflag &= ~(ICANON | ECHO);  // ICANON/ECHO在termios.h中定义：关闭规范模式+按键回显
    newt.c_cc[VMIN] = 0;               // VMIN在termios.h中定义：无按键时立即返回
    newt.c_cc[VTIME] = 0;              // VTIME在termios.h中定义：无超时等待
    if (tcsetattr(STDIN_FILENO, TCSANOW, &newt) != 0) {  // TCSANOW在termios.h中定义：立即应用设置
        tcsetattr(STDIN_FILENO, TCSANOW, &oldt);  // 恢复原有设置
        return 0;
    }

    // 3. 读取当前按键（read函数在unistd.h中定义）
    if (read(STDIN_FILENO, &ch, 1) > 0) {
        // 更新按键状态：按下设为true，急停键重置所有状态
        switch (ch) {
            case 'w': is_w_pressed = true; break;
            case 's': is_s_pressed = true; break;
            case 'a': is_a_pressed = true; break;
            case 'd': is_d_pressed = true; break;
            case 'x':  // 急停键：重置所有按键状态
                is_w_pressed = is_s_pressed = is_a_pressed = is_d_pressed = false;
                break;
            default:  // 其他按键：重置对应按键状态（避免长按残留）
                if (ch != 'w') is_w_pressed = false;
                if (ch != 's') is_s_pressed = false;
                if (ch != 'a') is_a_pressed = false;
                if (ch != 'd') is_d_pressed = false;
                break;
        }
    } else {
        // 无按键输入：重置所有按键状态（实现"松开即减速"）
        is_w_pressed = is_s_pressed = is_a_pressed = is_d_pressed = false;
    }

    // 4. 恢复终端原有设置（关键：避免终端卡住）
    tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
    return ch;
}

// 2. 速度限幅函数（防止超出自定义最大速度）
float ClampSpeed(float speed, float min_speed, float max_speed) {
    if (speed > max_speed) return max_speed;
    if (speed < min_speed) return min_speed;
    return speed;
}

// 3. 核心控制函数（包含while循环，需在main中调用）
void ScoutRobotControl(std::unique_ptr<ScoutRobot>& scout) {
    int count = 0;
    float linear_speed = 0.0f;  // 线速度（m/s，正=前进，负=后退）
    float angular_speed = 0.0f; // 角速度（rad/s，正=左转，负=右转）

    // 核心控制循环（修复原代码"while在全局 scope"的问题）
    while (true) {
        // 1. 读取按键状态
        char key = GetKeyWithState();

        // 2. 急停逻辑（按x立即归0）
        if (key == 'x') {
            linear_speed = 0.0f;
            angular_speed = 0.0f;
        } else {
            // 3. 线速度控制（w=前进，s=后退，松开减速）
            if (is_w_pressed && !is_s_pressed) {
                linear_speed = ClampSpeed(linear_speed + SPEED_STEP, -MAX_LINEAR_SPEED, MAX_LINEAR_SPEED);
            } else if (is_s_pressed && !is_w_pressed) {
                linear_speed = ClampSpeed(linear_speed - SPEED_STEP, -MAX_LINEAR_SPEED, MAX_LINEAR_SPEED);
            } else {
                // 松开w/s：减速至0
                if (linear_speed > 0.0f) {
                    linear_speed = std::max(0.0f, linear_speed - DECEL_STEP);
                } else if (linear_speed < 0.0f) {
                    linear_speed = std::min(0.0f, linear_speed + DECEL_STEP);
                }
            }

            // 4. 角速度控制（a=左转，d=右转，松开减速）
            if (is_a_pressed && !is_d_pressed) {
                angular_speed = ClampSpeed(angular_speed + SPEED_STEP * 2, -MAX_ANGULAR_SPEED, MAX_ANGULAR_SPEED);
            } else if (is_d_pressed && !is_a_pressed) {
                angular_speed = ClampSpeed(angular_speed - SPEED_STEP * 2, -MAX_ANGULAR_SPEED, MAX_ANGULAR_SPEED);
            } else {
                // 松开a/d：减速至0
                if (angular_speed > 0.0f) {
                    angular_speed = std::max(0.0f, angular_speed - DECEL_STEP * 2);
                } else if (angular_speed < 0.0f) {
                    angular_speed = std::min(0.0f, angular_speed + DECEL_STEP * 2);
                }
            }
        }

        // 5. 发送运动指令
        if (linear_speed != 0.0f || angular_speed != 0.0f) {
            scout->SetMotionCommand(linear_speed, angular_speed);
        }

        // 6. 原有状态读取与打印逻辑（完全保留）
        auto state = scout->GetRobotState();
        std::cout << "-------------------------------" << std::endl;
        std::cout << "count: " << count << std::endl;
        std::cout << "control mode: "
                  << static_cast<int>(state.system_state.control_mode)
                  << " , vehicle state: "
                  << static_cast<int>(state.system_state.vehicle_state)
                  << " , error code: " << std::hex << state.system_state.error_code
                  << std::dec
                  << ", battery voltage: " << state.system_state.battery_voltage
                  << std::endl;
        std::cout << "Current Command: linear=" << linear_speed 
                  << "m/s, angular=" << angular_speed << "rad/s" << std::endl;
        std::cout << "velocity (linear, angular): "
                  << state.motion_state.linear_velocity << ", "
                  << state.motion_state.angular_velocity << std::endl;
        std::cout << "core state age (ms): "
                  << duration_cast<milliseconds>(
                         SdkClock::now() - state.time_stamp)
                         .count()
                  << std::endl;

        auto actuator = scout->GetActuatorState();
        if (scout->GetParserProtocolVersion() == ProtocolVersion::AGX_V1) {
            for (int i = 0; i < 4; ++i) {
                printf("motor %d: current %f, rpm %d, driver temp %f, motor temp %f\n",
                       actuator.actuator_state[i].motor_id,
                       actuator.actuator_state[i].current,
                       actuator.actuator_state[i].rpm,
                       actuator.actuator_state[i].driver_temp,
                       actuator.actuator_state[i].motor_temp);
            }
            std::cout << "actuator state age (ms): "
                      << duration_cast<milliseconds>(
                             SdkClock::now() - actuator.time_stamp)
                             .count()
                      << std::endl;
        } else {
            for (int i = 0; i < 4; ++i) {
                printf("motor %d: current %f, rpm %d, driver temp %f, motor temp %f\n",
                       actuator.actuator_hs_state[i].motor_id,
                       actuator.actuator_hs_state[i].current,
                       actuator.actuator_hs_state[i].rpm,
                       actuator.actuator_ls_state[i].driver_temp,
                       actuator.actuator_ls_state[i].motor_temp);
            }
            std::cout << "actuator state age (ms): "
                      << duration_cast<milliseconds>(
                             SdkClock::now() - actuator.time_stamp)
                             .count()
                      << std::endl;
        }
        std::cout << "-------------------------------" << std::endl;

        // 7. 控制循环间隔（20ms）
        usleep(LOOP_INTERVAL_US);
        ++count;
    }
}

// -------------------------- main函数（小车初始化+启动控制）--------------------------
int main(int argc, char** argv) {
    // 1. 解析命令行参数（指定CAN接口，如can0）
    if (argc < 2) {
        std::cout << "Usage: ./demo_scout_robot <can_interface>" << std::endl;
        std::cout << "Example: ./demo_scout_robot can0" << std::endl;
        return -1;
    }
    std::string can_interface = argv[1];

    // 2. 初始化小车（修复std::make_unique的C++标准问题，若仍报错需在CMake指定C++14）
    std::unique_ptr<ScoutRobot> scout;
    ProtocolDetector detector;
    if (!detector.Connect(can_interface)) {
        std::cout << "Failed to connect to CAN interface: " << can_interface << std::endl;
        return -1;
    }

    // 3. 检测协议版本并创建小车实例
    auto proto = detector.DetectProtocolVersion(5);
    if (proto == ProtocolVersion::AGX_V1) {
        std::cout << "Detected protocol: AGX_V1" << std::endl;
        scout = std::unique_ptr<ScoutRobot>(new ScoutRobot(ProtocolVersion::AGX_V1, false));
        // 替代std::make_unique：若C++11环境，用new创建（避免C++14依赖）
    } else if (proto == ProtocolVersion::AGX_V2) {
        std::cout << "Detected protocol: AGX_V2" << std::endl;
        scout = std::unique_ptr<ScoutRobot>(new ScoutRobot(ProtocolVersion::AGX_V2, false));
        scout->EnableCommandedMode();  // V2协议需启用指令模式
    } else {
        std::cout << "Detected protocol: UNKNOWN" << std::endl;
        return -1;
    }

    // 4. 连接小车
    if (!scout->Connect(can_interface)) {
        std::cout << "Failed to connect to Scout robot" << std::endl;
        return -1;
    }
    std::cout << "Scout robot connected successfully!" << std::endl;
    std::cout << "Control Guide: w=前进, s=后退, a=左转, d=右转, x=急停" << std::endl;

    // 5. 启动核心控制循环（调用控制函数，避免while在全局）
    ScoutRobotControl(scout);

    // 6. 程序退出（理论上不会执行到这里，因控制循环是死循环）
    return 0;
}